package navigation;

import java.util.Iterator;
import java.util.List;

import movement.Engine;
import movement.Initialisation;
import protocol.Protocol.Direction;
import protocol.Protocol.Field;
import sensors.Grid;
import sensors.Touch;

public class Navigation {

	Initialisation init;
	public static Direction currentDirection;
	Direction lastDirection;
	Engine engine;
	Grid grid;
	public static Touch touch;

	/**
	 * The main Navigation class. (Don't instantiate their subclasses) The
	 * instantiation of this class instantiates and connects all their
	 * subclasses and provides the Navigation Methods.
	 * 
	 * @param one
	 *            Diameter of the tire
	 * @param two
	 *            Distance between center of right tire and center of left tire,
	 *            in same units as wheelDiameter
	 */
	public Navigation(double one, double two) {
		this.init = new Initialisation(one, two);
		this.engine = init.getEngine();
		touch = init.getTouch();
		this.grid = init.getGrid();

		currentDirection = init.getDirection();
	}

	// public Field[] getNeighbours() {
	//
	// Direction[] directions = { Direction.NORTH, Direction.EAST,
	// Direction.SOUTH, Direction.WEST };
	//
	// Field[] result = new Field[4];
	//
	// if (lastDirection == null) {
	// for (int i = 0; i < result.length; i++) {
	// result[i] = moveTo(directions[i]);
	// }
	//
	// } else {
	// if (lastDirection == Direction.NORTH) {
	// result[0] = Field.Free;
	// result[1] = moveTo(Direction.EAST);
	// result[2] = moveTo(Direction.SOUTH);
	// result[3] = moveTo(Direction.WEST);
	// } else if (lastDirection == Direction.EAST) {
	// result[0] = moveTo(Direction.NORTH);
	// result[1] = Field.Free;
	// result[2] = moveTo(Direction.SOUTH);
	// result[3] = moveTo(Direction.WEST);
	// } else if (lastDirection == Direction.SOUTH) {
	// result[0] = moveTo(Direction.NORTH);
	// result[1] = moveTo(Direction.EAST);
	// result[2] = Field.Free;
	// result[3] = moveTo(Direction.WEST);
	// } else {
	// result[0] = moveTo(Direction.NORTH);
	// result[1] = moveTo(Direction.EAST);
	// result[2] = moveTo(Direction.SOUTH);
	// result[3] = Field.Free;
	// }
	// }
	// return result;
	//
	// }

	/**
	 * Performs a move into the desired Direction
	 * 
	 * @param direction
	 *            The desired Direction
	 * @return the type of the Field in the desired Direction
	 */
	private Field moveTo(Direction direction) {

		realign(direction);
		return engine.explore(true);

	}

	/**
	 * Performs a simple move in the desired Direction without exploring
	 * 
	 * @param direction
	 *            The Direction the Agent should move
	 */
	public void simpleMoveTo(Direction direction) {

		lastDirection = currentDirection;
		realign(direction);
		currentDirection = direction;
		engine.explore(false);

	}

	/**
	 * The Agent turns into the new direction
	 * 
	 * @param direction
	 *            the desired Direction
	 */
	private void realign(Direction direction) {

		switch (direction) {
		case NORTH:
			switch (currentDirection) {
			case NORTH:
				lastDirection = Direction.NORTH;
				break;
			case EAST:
				engine.rotate(-1 * 75);
				currentDirection = Direction.NORTH;
				lastDirection = Direction.EAST;
				break;
			case SOUTH:
				engine.rotate(2 * 75);
				currentDirection = Direction.NORTH;
				lastDirection = Direction.SOUTH;
				break;
			case WEST:
				engine.rotate(1 * 75);
				currentDirection = Direction.NORTH;
				lastDirection = Direction.WEST;
				break;

			default:
				break;
			}
			break;

		case EAST:
			switch (currentDirection) {
			case NORTH:
				engine.rotate(1 * 75);
				currentDirection = Direction.EAST;
				lastDirection = Direction.NORTH;
				break;
			case EAST:
				lastDirection = Direction.EAST;
				break;
			case SOUTH:
				engine.rotate(-1 * 75);
				currentDirection = Direction.EAST;
				lastDirection = Direction.SOUTH;
				break;
			case WEST:
				engine.rotate(2 * 75);
				currentDirection = Direction.EAST;
				lastDirection = Direction.WEST;
				break;

			default:
				break;
			}
			break;
		case SOUTH:
			switch (currentDirection) {
			case NORTH:
				engine.rotate(2 * 75);
				currentDirection = Direction.SOUTH;
				lastDirection = Direction.NORTH;
				break;
			case EAST:
				engine.rotate(1 * 75);
				currentDirection = Direction.SOUTH;
				lastDirection = Direction.EAST;
				break;
			case SOUTH:
				lastDirection = Direction.SOUTH;
				break;
			case WEST:
				engine.rotate(-1 * 75);
				currentDirection = Direction.SOUTH;
				lastDirection = Direction.WEST;
				break;

			default:
				break;
			}
			break;
		case WEST:
			switch (currentDirection) {
			case NORTH:
				engine.rotate(-1 * 75);
				currentDirection = Direction.WEST;
				lastDirection = Direction.NORTH;
				break;
			case EAST:
				engine.rotate(2 * 75);
				currentDirection = Direction.WEST;
				lastDirection = Direction.EAST;
				break;
			case SOUTH:
				engine.rotate(1 * 75);
				currentDirection = Direction.WEST;
				lastDirection = Direction.SOUTH;
				break;
			case WEST:
				lastDirection = Direction.WEST;
				break;

			default:
				break;
			}
			break;

		default:
			break;
		}

	}

	/**
	 * Explores the desired fields and returns their type
	 * 
	 * @param myList
	 *            List of Directions to explore
	 * @return An Array containing the types of the explored fields (in the same
	 *         order as the input)
	 */
	public Field[] lookAround(List<Direction> myList) {

		Field[] result = new Field[myList.size()];
		int index = 0;

		for (Iterator<Direction> iterator = myList.iterator(); iterator
				.hasNext();) {
			Direction object = (Direction) iterator.next();

			result[index] = moveTo(object);
			index++;

		}

		return result;
	}
}
